//==============================================================================
// #include <xmc4100.h>

// #include <xmc_ccu8.h>
// #include <xmc_gpio.h>
// #include <xmc_scu.h>
// #include <xmc_vadc.h>

// #include <math.h>
// #include <stdlib.h>

// #define  ARM_MATH_CM4
// #include <arm_math.h>
//==============================================================================
// #include "SEGGER_RTT.h"

#include "main.h"
#include "yb_foc.h"
#include "foc_Qtype.h"

uint8_t    rtt_buf1[1024];
#pragma    pack(push, 1)
struct
{
	// float var1;
	// float var2;
	// float var3;
	// float var4;	

	short var1;
	short var2;
	short var3;
	short var4;
} varbuf;
#pragma pack(pop)
//==============================================================================

void CCU80_IO_PWM_init (void);
void CCU80_IO_GPIO_init (void);

//==============================================================================
#define MODULE_PTR      	CCU80
#define MODULE_NUMBER   	(0U)

#define SLICE0_PTR       	CCU80_CC80
#define SLICE0_NUMBER    	(0U)
#define SLICE0_OUTPUT00		P0_5
#define SLICE0_OUTPUT01		P0_2

#define SLICE1_PTR       	CCU80_CC81
#define SLICE1_NUMBER    	(1U)
#define SLICE1_OUTPUT10		P0_4
#define SLICE1_OUTPUT11		P0_1

#define SLICE2_PTR       	CCU80_CC82
#define SLICE2_NUMBER    	(2U)
#define SLICE2_OUTPUT20		P0_3
#define SLICE2_OUTPUT21		P0_0

#define SLICE3_PTR       	CCU80_CC83
#define SLICE3_NUMBER    	(3U)

//==============================================================================

//==============================================================================
uint16_t   	pwm_period = 1999;

uint16_t   	pwm_duty_a = 1000;
uint16_t   	pwm_duty_b = 1000;
uint16_t   	pwm_duty_c = 1000;

uint16_t   	vadc_ia, vadc_ib, vadc_ic, vadc_vpot;

uint32_t	adc_isr_flag, opa_cali_flag;
//==============================================================================
uint16_t	ia_offset, ib_offset, ic_offset;
int16_t		ia_mea, ib_mea, ic_mea;//mea--测量值

float		ia_mea_f, ib_mea_f, ic_mea_f;
short		ia_mea_t, ib_mea_t, ic_mea_t;

float		Ialpha, Ibeta;
short		Ialpha_t, Ibeta_t;

float		Id_mea, Iq_mea;
short		Id_mea_t, Iq_mea_t;

float		Valpha = 0.0;   
float		Vbeta = 0.0;

volatile float Vd = 0.0;   
volatile float Vq = 0.0;   

float		theta;
float		theta_inc;
float		theta_est;

volatile uint32_t cnt_main = 0;
volatile char cnt_ms = 0;
volatile char b_ms = 0;

float sin_f,cos_f;
short sin_t,cos_t;
int tmp1, tmp2;

//---------------------------------------------------------------------
#if MOTOR_SEL == M_A2212
	//电流环参数
	float		Id_ref = 0.0f;
	float		Id_err = 0.0f;
	float		Kp_d = 0.02f;//0.09f;id
	float		Ki_d = 0.01f;//0.04f;	id
	float		Kc_d = 0.01f;//0.04f;	id
	float		sum_d = 0.0f; 

	float		Iq_ref = 0.0f;
	float		Iq_err = 0.0f;
	float		Kp_q = 0.02f;//0.09f;id
	float		Ki_q = 0.01f;//0.04f;	id
	float		Kc_q = 0.01f;//0.04f;id
	float		sum_q = 0.0f; 

	//速度环参数
	float		sp_err = 0.0f;
	float		KcErr_sp;	
	float		Kp_sp = 0.09f;	
	float		Ki_sp = 0.04f;	
	float		Kc_sp = 0.04f;
	float		sum_sp = 0.0f; 
	float		omega_val;
#elif MOTOR_SEL == LA034
	//电流环参数
	#define ID_KP	(0.01f)
	#define ID_KI	(0.01f)
	float		Id_ref = 0.0f;
	float		Id_err = 0.0f;
	float		Kp_d = ID_KP;
	float		Ki_d = ID_KI;	
	float		Kc_d = ID_KI;	
	float		sum_d = 0.0f; 

	#define IQ_KP	(0.01f)
	#define IQ_KI	(0.01f)
	float		Iq_ref = 0.0f;
	float		Iq_err = 0.0f;
	float		Kp_q = IQ_KP;
	float		Ki_q = IQ_KI;	
	float		Kc_q = IQ_KI;
	float		sum_q = 0.0f; 

	//速度环参数
	#define SP_KP	(0.005f)
	#define SP_KI	(0.02f)
	float		sp_err = 0.0f;
	float		KcErr_sp;	
	float		Kp_sp = SP_KP;	
	float		Ki_sp = SP_KI;	
	float		Kc_sp = SP_KI;
	float		sum_sp = 0.0f; 
	float		omega_val;
#elif MOTOR_SEL == CUR
	//电流环参数
	#define ID_KP	(0.01f)
	#define ID_KI	(0.01f)
	float		Id_ref = 0.0f;
	float		Id_err = 0.0f;
	float		Kp_d = ID_KP;
	float		Ki_d = ID_KI;	
	float		Kc_d = ID_KI;	
	float		sum_d = 0.0f; 

	#define IQ_KP	(0.02f)
	#define IQ_KI	(0.01f)
	float		Iq_ref = 0.0f;
	float		Iq_err = 0.0f;
	float		Kp_q = IQ_KP;
	float		Ki_q = IQ_KI;	
	float		Kc_q = IQ_KI;
	float		sum_q = 0.0f; 

	//速度环参数
	#define SP_KP	(0.02f)
	#define SP_KI	(0.05f)
	float		sp_err = 0.0f;
	float		KcErr_sp;	
	float		Kp_sp = SP_KP;	
	float		Ki_sp = SP_KI;	
	float		Kc_sp = SP_KI;
	float		sum_sp = 0.0f; 
	float		omega_val;
#elif MOTOR_SEL == GUN
	//电流环参数
	#define ID_KP	(0.02f)
	#define ID_KI	(0.01f)
	float		Id_ref = 0.0f;
	float		Id_err = 0.0f;
	float		Kp_d = ID_KP;
	float		Ki_d = ID_KI;	
	float		Kc_d = ID_KI;	
	float		sum_d = 0.0f; 

	#define IQ_KP	(0.02f)
	#define IQ_KI	(0.01f)
	float		Iq_ref = 0.0f;
	float		Iq_err = 0.0f;
	float		Kp_q = IQ_KP;
	float		Ki_q = IQ_KI;	
	float		Kc_q = IQ_KI;
	float		sum_q = 0.0f; 

	//速度环参数
	#define SP_KP	(0.02f)
	#define SP_KI	(0.01f)
	float		sp_err = 0.0f;
	float		KcErr_sp;	
	float		Kp_sp = SP_KP;	
	float		Ki_sp = SP_KI;	
	float		Kc_sp = SP_KI;
	float		sum_sp = 0.0f; 
	float		omega_val;
#endif


//---------------------------------------------------------------------
float		Ed = 0.0f;
float		Eq = 0.0f; 
float       e_omega_set = 0.0f;
float		e_omega_est = 0.0f;
float       e_omega_flt = 0.0f;
float 		sum_omega;
float 		lv_coe = 256.0f;

float   	R = RS;
float		L = LS;
float		Kp_pll = 2000.0f;				
float		Ki_pll = 10.0f;					
float		sum_pll = 0.0f;
float		theta_err = 0.0f;
float		theta_err_limit = 1.0471f;		//
//---------------------------------------------------------------------
float		Va_abs, Vb3_abs;
float		d1, d2, d3, d4, d5, d6, d7;
float		da, db, dc;
float		Vbat = VDC;
float		K_svpwm 	= 1.5f;	//
float		two_sqrt3 	= 1.154700538f;
float		one_sqrt3  	= 0.5773502692f;
//---------------------------------------------------------------------
volatile	uint16_t	mode;
uint16_t	delay_cnt;
uint16_t	ramp_cnt;
uint16_t	ramp_val;
volatile	uint16_t	sw1_flag;
uint32_t	chk_cnt;
uint16_t	led_cnt;
uint16_t	led_flag;

//==============================================================================


void svpwm (void)
{
	//----------------------------------------------------------------- 	
	Va_abs  = fabs(Valpha);
	Vb3_abs = fabs(Vbeta * one_sqrt3);
	  
	if ( (Valpha >= 0.0f) && (Vbeta >= 0.0f) && (Va_abs >= Vb3_abs) ){  //sect 1		
		//K_svpwm = 3/2
		d4 = K_svpwm * (Valpha - one_sqrt3 * Vbeta) / Vbat;//V4 鏃堕棿 V4/Vmax   Vmax = 2/3Vbat  
		d6 = K_svpwm * (    two_sqrt3 * Vbeta) / Vbat;//V6 鏃堕棿
		d7 = (1.0f - (d4 + d6)) * 0.5f;//V7 鏃堕棿  (1.0f - (d4 + d6)) == V0 + V7

		da = d4 + d6 + d7;//A鐩搁珮鐢靛钩鍗犵┖锟??
		db =      d6 + d7;//B鐩搁珮鐢靛钩鍗犵┖锟??
		dc =           d7;//C鐩搁珮鐢靛钩鍗犵┖锟??
	}

	if ( (Vbeta > 0.0f) && (Va_abs <= one_sqrt3 * Vbeta) ){  			//sect 2
		d6 = K_svpwm * ( Valpha + one_sqrt3 * Vbeta) / Vbat;
		d2 = K_svpwm * (-Valpha + one_sqrt3 * Vbeta) / Vbat;
		d7 = (1.0f - (d6 + d2)) * 0.5f;

		da =      d6 + d7;
		db = d6 + d2 + d7;
		dc =           d7;
	} 
	 
	if ( (Valpha <= 0.0f) && (Vbeta >= 0.0f) && (Va_abs >= Vb3_abs)){  //sect 3    
		d2 = K_svpwm * two_sqrt3 * Vbeta / Vbat;
		d3 = K_svpwm * (-Valpha - one_sqrt3 * Vbeta) / Vbat;
		d7 = (1.0f - (d2 + d3)) * 0.5f;

		da =           d7;
		db = d2 + d3 + d7;
		dc =      d3 + d7;
	} 
 
	if ( (Valpha <= 0.0f) && (Vbeta <= 0.0f) && (Va_abs >= Vb3_abs) ){  //sect 4     
		d1 = -K_svpwm * two_sqrt3 * Vbeta / Vbat;
		d3 =  K_svpwm * (-Valpha + one_sqrt3 * Vbeta) / Vbat;
		d7 = (1.0f - (d3 + d1)) * 0.5f;

		da =           d7;
		db =      d3 + d7;
		dc = d3 + d1 + d7;
	}  

	if ( (Va_abs <= -one_sqrt3 * Vbeta) ){  //sect 5
		d1 = K_svpwm * (-Valpha - one_sqrt3 * Vbeta) / Vbat;
		d5 = K_svpwm * ( Valpha - one_sqrt3 * Vbeta) / Vbat;
		d7 = (1.0f - (d1 + d5)) * 0.5f;

		da =      d5 + d7;
		db =           d7;
		dc = d1 + d5 + d7;
	}  
 
	if ( (Valpha >= 0.0f) && (Vbeta <= 0.0f) && (Va_abs >= Vb3_abs) ){  //sect 6  
		d4 =  K_svpwm * (Valpha + one_sqrt3 * Vbeta) / Vbat;
		d5 = -K_svpwm * two_sqrt3 * Vbeta / Vbat;
		d7 = (1.0f - (d4 + d5)) * 0.5f;

		da = d4 + d5 + d7;
		db =           d7;
		dc =      d5 + d7;
	}
	
	//-----------------------------------------------------------------
	CCU80_CC80->CR1S = 2000 - (uint16_t)(2000 * da);
	CCU80_CC81->CR1S = 2000 - (uint16_t)(2000 * db);
	CCU80_CC82->CR1S = 2000 - (uint16_t)(2000 * dc);

	//-----------------------------------------------------------------
	CCU80->GCSS |= 0X00000111;
	
	//-----------------------------------------------------------------
}
//==============================================================================
XMC_SCU_CLOCK_CONFIG_t clock_config = 
{
	.syspll_config.n_div = 20U,
	.syspll_config.p_div = 1U,
	.syspll_config.k_div = 4U,

	.syspll_config.mode = XMC_SCU_CLOCK_SYSPLL_MODE_NORMAL,
	.syspll_config.clksrc = XMC_SCU_CLOCK_SYSPLLCLKSRC_OSCHP,
	.enable_oschp = true,

	.enable_osculp = false,

	.calibration_mode = XMC_SCU_CLOCK_FOFI_CALIBRATION_MODE_FACTORY,
	.fstdby_clksrc = XMC_SCU_HIB_STDBYCLKSRC_OSI,
	.fsys_clksrc = XMC_SCU_CLOCK_SYSCLKSRC_PLL,
	.fsys_clkdiv = 1U,
	.fcpu_clkdiv = 1U,
	.fccu_clkdiv = 1U,
	.fperipheral_clkdiv = 1U
};

//==============================================================================
XMC_CCU8_SLICE_COMPARE_CONFIG_t  SLICE_config     =
{
	.timer_mode            = (uint32_t)XMC_CCU8_SLICE_TIMER_COUNT_MODE_CA,
	.monoshot              = (uint32_t)XMC_CCU8_SLICE_TIMER_REPEAT_MODE_REPEAT,
	.shadow_xfer_clear     = 0U,
	.dither_timer_period   = 0U,
	.dither_duty_cycle     = 0U,

	.prescaler_mode        = (uint32_t)XMC_CCU8_SLICE_PRESCALER_MODE_NORMAL,

	.mcm_ch1_enable        = 0U,
	.mcm_ch2_enable        = 0U,

	.slice_status          = (uint32_t)XMC_CCU8_SLICE_STATUS_CHANNEL_1,

	.passive_level_out0    = (uint32_t)XMC_CCU8_SLICE_OUTPUT_PASSIVE_LEVEL_LOW,
	.passive_level_out1    = (uint32_t)XMC_CCU8_SLICE_OUTPUT_PASSIVE_LEVEL_LOW,

	.asymmetric_pwm        = 0U,

	.invert_out0           = 0U,
	.invert_out1           = 1U,
	.invert_out2           = 0U,
	.invert_out3           = 1U,

	.prescaler_initval     = 0U,
	.float_limit           = 0U,
	.dither_limit          = 0U,
	.timer_concatenation   = 0U,
};


XMC_CCU8_SLICE_EVENT_CONFIG_t SLICE_event0_config =
{
	.mapped_input        = XMC_CCU8_SLICE_INPUT_H,			//Connected to SCU.GSC80
	.edge                = XMC_CCU8_SLICE_EVENT_EDGE_SENSITIVITY_RISING_EDGE,
	.level               = XMC_CCU8_SLICE_EVENT_LEVEL_SENSITIVITY_ACTIVE_LOW,
	.duration            = XMC_CCU8_SLICE_EVENT_FILTER_DISABLED,
};
//==============================================================================
XMC_CCU8_SLICE_DEAD_TIME_CONFIG_t  SLICE_dt_config =
{
	.enable_dead_time_channel1         = 1U,
	.channel1_st_path                  = 1U,
	.channel1_inv_st_path              = 1U,
	.div                               = (uint32_t)XMC_CCU8_SLICE_DTC_DIV_1,
	.channel1_st_rising_edge_counter   = 40U,	    // 500ns
	.channel1_st_falling_edge_counter  = 40U,     	// 500ns
};
//==============================================================================
XMC_GPIO_CONFIG_t  OUTPUT_strong_sharp_config =
{
	.mode                = XMC_GPIO_MODE_OUTPUT_PUSH_PULL,
	.output_level        = XMC_GPIO_OUTPUT_LEVEL_LOW,
	.output_strength     = XMC_GPIO_OUTPUT_STRENGTH_STRONG_SHARP_EDGE
};  
//==============================================================================
const XMC_VADC_GLOBAL_CONFIG_t g_global_handle = 
{
    .clock_config = 
	{
        .analog_clock_divider = 3,
    },
};

const XMC_VADC_SCAN_CONFIG_t g_scan_handle = 
{
    .trigger_signal = XMC_VADC_REQ_TR_I,
    .trigger_edge = XMC_VADC_TRIGGER_EDGE_ANY,
    .external_trigger = 1,
    .req_src_interrupt = 1, 
};

const XMC_VADC_CHANNEL_CONFIG_t g_g0_ch0_handle = 
{
    .alias_channel = XMC_VADC_CHANNEL_ALIAS_DISABLED,
    .result_reg_number = 0, 
};

const XMC_VADC_CHANNEL_CONFIG_t g_g0_ch3_handle = 
{
    .alias_channel = XMC_VADC_CHANNEL_ALIAS_DISABLED,
    .result_reg_number = 3, 
};

const XMC_VADC_CHANNEL_CONFIG_t g_g0_ch4_handle = 
{
    .alias_channel = XMC_VADC_CHANNEL_ALIAS_DISABLED,
    .result_reg_number = 4, 
};

const XMC_VADC_CHANNEL_CONFIG_t g_g0_ch6_handle = 
{
    .alias_channel = XMC_VADC_CHANNEL_ALIAS_DISABLED,
    .result_reg_number = 6, 
};
//******************************************************************************
void CCU80_0_IRQHandler (void)
{	
	
}
//******************************************************************************

//==============================================================================
const XMC_GPIO_CONFIG_t  user_switch_read_config =
{
	.mode                = XMC_GPIO_MODE_INPUT_TRISTATE
};
//==============================================================================
XMC_GPIO_CONFIG_t  CCU8_strong_soft_config =
{
	.mode                = XMC_GPIO_MODE_OUTPUT_PUSH_PULL_ALT3,
	.output_level        = XMC_GPIO_OUTPUT_LEVEL_LOW,
	.output_strength     = XMC_GPIO_OUTPUT_STRENGTH_STRONG_SOFT_EDGE
};
//==============================================================================
XMC_GPIO_CONFIG_t  CCU8_strong_sharp_config =
{
	.mode                = XMC_GPIO_MODE_OUTPUT_PUSH_PULL_ALT3,
	.output_level        = XMC_GPIO_OUTPUT_LEVEL_LOW,
	.output_strength     = XMC_GPIO_OUTPUT_STRENGTH_STRONG_SHARP_EDGE
};
//==============================================================================
XMC_GPIO_CONFIG_t  CCU8_gpio_strong_soft_config =
{
	.mode                = XMC_GPIO_MODE_OUTPUT_PUSH_PULL,
	.output_level        = XMC_GPIO_OUTPUT_LEVEL_LOW,
	.output_strength     = XMC_GPIO_OUTPUT_STRENGTH_STRONG_SOFT_EDGE
};
//==============================================================================
XMC_GPIO_CONFIG_t  CCU8_gpio_strong_sharp_config =
{
	.mode                = XMC_GPIO_MODE_OUTPUT_PUSH_PULL,
	.output_level        = XMC_GPIO_OUTPUT_LEVEL_LOW,
	.output_strength     = XMC_GPIO_OUTPUT_STRENGTH_STRONG_SHARP_EDGE
};
//==============================================================================
void CCU80_IO_PWM_init (void)
{	
	XMC_GPIO_Init(P0_5, &CCU8_strong_sharp_config);
	XMC_GPIO_Init(P0_4, &CCU8_strong_sharp_config);	
	XMC_GPIO_Init(P0_3, &CCU8_strong_sharp_config);	
	XMC_GPIO_Init(P0_2, &CCU8_strong_sharp_config);	
	XMC_GPIO_Init(P0_1, &CCU8_strong_soft_config);	
	XMC_GPIO_Init(P0_0, &CCU8_strong_soft_config);	
}
//==============================================================================
void CCU80_IO_GPIO_init (void)
{
	XMC_GPIO_Init(P0_5, &CCU8_gpio_strong_sharp_config);
	XMC_GPIO_Init(P0_4, &CCU8_gpio_strong_sharp_config);	
	XMC_GPIO_Init(P0_3, &CCU8_gpio_strong_sharp_config);	
	XMC_GPIO_Init(P0_2, &CCU8_gpio_strong_sharp_config);	
	XMC_GPIO_Init(P0_1, &CCU8_gpio_strong_soft_config);	
	XMC_GPIO_Init(P0_0, &CCU8_gpio_strong_soft_config);	
}
//==============================================================================
void delay_100_us (void)
{
	uint16_t	m;
	for (m = 0; m < 1600; m++);
}
//==============================================================================
void delay_ms (uint16_t	m)
{
	uint16_t	p, q;
	for (p = 0; p < m; p++)
	{
		for (q = 0; q < 10; q++)
		{
			delay_100_us ();
		}
	}
}
//==============================================================================
void CCU80_init (void)
{
	/* Enable clock, enable prescaler block and configure global control */
	XMC_CCU8_Init(MODULE_PTR, XMC_CCU8_SLICE_MCMS_ACTION_TRANSFER_PR_CR);

	/* Start the prescaler and restore clocks to slices */
	XMC_CCU8_StartPrescaler(MODULE_PTR);

	/* Start of CCU8 configurations */
	/* Ensure fCCU reaches CCU80 */
	XMC_CCU8_SetModuleClock(MODULE_PTR, XMC_CCU8_CLOCK_SCU);

	/* Configure CCU8x_CC8y slice as timer */
	XMC_CCU8_SLICE_CompareInit(SLICE0_PTR, &SLICE_config);
	XMC_CCU8_SLICE_CompareInit(SLICE1_PTR, &SLICE_config);
	XMC_CCU8_SLICE_CompareInit(SLICE2_PTR, &SLICE_config);
	XMC_CCU8_SLICE_CompareInit(SLICE3_PTR, &SLICE_config);
	/* Set period match value of the timer  */
	XMC_CCU8_SLICE_SetTimerPeriodMatch(SLICE0_PTR, pwm_period);
	XMC_CCU8_SLICE_SetTimerPeriodMatch(SLICE1_PTR, pwm_period);
	XMC_CCU8_SLICE_SetTimerPeriodMatch(SLICE2_PTR, pwm_period);
	XMC_CCU8_SLICE_SetTimerPeriodMatch(SLICE3_PTR, pwm_period);
	
	/* Set timer compare match value for channel 1 - (80%, 60%, 30%) Duty cycle */
	XMC_CCU8_SLICE_SetTimerCompareMatch(SLICE0_PTR, XMC_CCU8_SLICE_COMPARE_CHANNEL_1, pwm_duty_a);
	XMC_CCU8_SLICE_SetTimerCompareMatch(SLICE1_PTR, XMC_CCU8_SLICE_COMPARE_CHANNEL_1, pwm_duty_b);
	XMC_CCU8_SLICE_SetTimerCompareMatch(SLICE2_PTR, XMC_CCU8_SLICE_COMPARE_CHANNEL_1, pwm_duty_c);
	XMC_CCU8_SLICE_SetTimerCompareMatch(SLICE3_PTR, XMC_CCU8_SLICE_COMPARE_CHANNEL_1, 1);
	
	/* Transfer value from shadow timer registers to actual timer registers */
	XMC_CCU8_EnableShadowTransfer(MODULE_PTR, XMC_CCU8_SHADOW_TRANSFER_SLICE_0);
	XMC_CCU8_EnableShadowTransfer(MODULE_PTR, XMC_CCU8_SHADOW_TRANSFER_SLICE_1);
	XMC_CCU8_EnableShadowTransfer(MODULE_PTR, XMC_CCU8_SHADOW_TRANSFER_SLICE_2);
	XMC_CCU8_EnableShadowTransfer(MODULE_PTR, XMC_CCU8_SHADOW_TRANSFER_SLICE_3);
	/* Configure events */
	XMC_CCU8_SLICE_ConfigureEvent(SLICE0_PTR, XMC_CCU8_SLICE_EVENT_0, &SLICE_event0_config);
	XMC_CCU8_SLICE_ConfigureEvent(SLICE1_PTR, XMC_CCU8_SLICE_EVENT_0, &SLICE_event0_config);
	XMC_CCU8_SLICE_ConfigureEvent(SLICE2_PTR, XMC_CCU8_SLICE_EVENT_0, &SLICE_event0_config);
	XMC_CCU8_SLICE_ConfigureEvent(SLICE3_PTR, XMC_CCU8_SLICE_EVENT_0, &SLICE_event0_config);

	XMC_CCU8_SLICE_StartConfig(SLICE0_PTR, XMC_CCU8_SLICE_EVENT_0, XMC_CCU8_SLICE_START_MODE_TIMER_START_CLEAR);
	XMC_CCU8_SLICE_StartConfig(SLICE1_PTR, XMC_CCU8_SLICE_EVENT_0, XMC_CCU8_SLICE_START_MODE_TIMER_START_CLEAR);
	XMC_CCU8_SLICE_StartConfig(SLICE2_PTR, XMC_CCU8_SLICE_EVENT_0, XMC_CCU8_SLICE_START_MODE_TIMER_START_CLEAR);
	XMC_CCU8_SLICE_StartConfig(SLICE3_PTR, XMC_CCU8_SLICE_EVENT_0, XMC_CCU8_SLICE_START_MODE_TIMER_START_CLEAR);

	XMC_CCU8_SLICE_EnableEvent(SLICE3_PTR, XMC_CCU8_SLICE_IRQ_ID_ONE_MATCH);
	//--------------------------------------------------------
	XMC_CCU8_SLICE_EnableEvent(CCU80_CC83, XMC_CCU8_SLICE_IRQ_ID_COMPARE_MATCH_UP_CH_1);

	XMC_CCU8_SLICE_SetInterruptNode(CCU80_CC83, XMC_CCU8_SLICE_IRQ_ID_COMPARE_MATCH_UP_CH_1, XMC_CCU8_SLICE_SR_ID_2);
	/* Connect one match event to SR1 */
	XMC_CCU8_SLICE_SetInterruptNode(SLICE3_PTR, \
	XMC_CCU8_SLICE_IRQ_ID_ONE_MATCH, XMC_CCU8_SLICE_SR_ID_0);

	/* Set NVIC priority */
	NVIC_SetPriority(CCU80_0_IRQn, 60U);

	/* Enable IRQ */
	//NVIC_EnableIRQ(CCU80_0_IRQn);

	/* Deadtime initialisation*/
	XMC_CCU8_SLICE_DeadTimeInit(SLICE0_PTR, &SLICE_dt_config);
	XMC_CCU8_SLICE_DeadTimeInit(SLICE1_PTR, &SLICE_dt_config);
	XMC_CCU8_SLICE_DeadTimeInit(SLICE2_PTR, &SLICE_dt_config);
	XMC_CCU8_SLICE_DeadTimeInit(SLICE3_PTR, &SLICE_dt_config);
	
	/* Get the slice out of idle mode */
	XMC_CCU8_EnableClock(MODULE_PTR, SLICE0_NUMBER);
	XMC_CCU8_EnableClock(MODULE_PTR, SLICE1_NUMBER);
	XMC_CCU8_EnableClock(MODULE_PTR, SLICE2_NUMBER);
	XMC_CCU8_EnableClock(MODULE_PTR, SLICE3_NUMBER);

	/* Start the PWM on a rising edge on SCU.GSC80 */
	XMC_SCU_SetCcuTriggerHigh(XMC_SCU_CCU_TRIGGER_CCU80);

	CCU80_IO_PWM_init ();	
}
//==============================================================================
void ADC_init (void)
{
    /* Provide clock to VADC and initialize the VADC global registers. */
    XMC_VADC_GLOBAL_Init(VADC, &g_global_handle);

    /* Set VADC group to normal operation mode (VADC kernel). */
    XMC_VADC_GROUP_SetPowerMode(VADC_G0, XMC_VADC_GROUP_POWERMODE_NORMAL);

    /* Calibrate the VADC. Make sure you do this after all used VADC groups
     * are set to normal operation mode. */
    XMC_VADC_GLOBAL_StartupCalibration(VADC);

    /* Initialize the scan source hardware.  The gating mode is set to
     * ignore to pass external triggers unconditionally. */
    XMC_VADC_GROUP_ScanInit(VADC_G0, &g_scan_handle);

    /* Initialize the channel units. */
    XMC_VADC_GROUP_ChannelInit(VADC_G0, 0, &g_g0_ch0_handle);
    XMC_VADC_GROUP_ChannelInit(VADC_G0, 3, &g_g0_ch3_handle);
    XMC_VADC_GROUP_ChannelInit(VADC_G0, 4, &g_g0_ch4_handle);
	XMC_VADC_GROUP_ChannelInit(VADC_G0, 6, &g_g0_ch6_handle);
    /* Add channels to the scan source. */
    XMC_VADC_GROUP_ScanAddChannelToSequence(VADC_G0, 0);
    XMC_VADC_GROUP_ScanAddChannelToSequence(VADC_G0, 3);
    XMC_VADC_GROUP_ScanAddChannelToSequence(VADC_G0, 4);
	XMC_VADC_GROUP_ScanAddChannelToSequence(VADC_G0, 6);
}
//==============================================================================
void get_current_offset (void)
{
	uint16_t	sum_a, sum_b, sum_c;
	uint16_t	cnt;
	
	adc_isr_flag = 0;
	opa_cali_flag = 0;
	
	sum_a = 0;
	sum_b = 0;
	
	for (cnt = 0; cnt < 8; cnt++){
		while ( !adc_isr_flag );
		adc_isr_flag = 0;
		
		sum_a += XMC_VADC_GROUP_GetResult(VADC_G0, 0);
		sum_b += XMC_VADC_GROUP_GetResult(VADC_G0, 3);
		sum_c += XMC_VADC_GROUP_GetResult(VADC_G0, 4);
	}
	
	ia_offset = sum_a >> 3;
	ib_offset = sum_b >> 3;
	ic_offset = sum_c >> 3;
	
	opa_cali_flag = 1;
	
	delay_ms (10);
}

//==============================================================================
uint16_t key_delay = 0;
uint8_t key = 0;

void foc_status(void)
{
	switch (mode){
		case e_idle:
			sum_sp = 0;
			sp_err = 0;
			LED_SIMLL_BOARD_SET(0);
			if (++led_cnt == 100){
				led_cnt = 0;

				led_flag ^= 1;
				if (led_flag){
					LED_BIG_BOARD_SET(0);
				}else{
					LED_BIG_BOARD_SET(1);
				}
			}
			break;
		
		case e_align:
		
			break;

		case e_open:

			break;

		case e_close:
			speed_loop();

			LED_BIG_BOARD_SET(0);
			if (++led_cnt == 100){
				led_cnt = 0;

				led_flag ^= 1;
				if (led_flag){
					LED_SIMLL_BOARD_SET(0);
				}else{
					LED_SIMLL_BOARD_SET(1);
				}
			}	

			break;

		case e_err:
		
			break;
		default:break;
	}
}

int main (void)
{
	static uint8_t b_inv = 0;
	
    //---------------------------------------------------------
	XMC_SCU_CLOCK_Init(&clock_config);
	delay_ms (50);

	//---------------------------------------------------------
	#if SEGGER_EN == 1
		// SEGGER_RTT_ConfigUpBuffer (1, "JScope_f4f4f4", rtt_buf1, sizeof(rtt_buf1), SEGGER_RTT_MODE_NO_BLOCK_SKIP);
		SEGGER_RTT_ConfigUpBuffer (1, "JScope_i4i4i4i4", rtt_buf1, sizeof(rtt_buf1), SEGGER_RTT_MODE_NO_BLOCK_SKIP);
		SEGGER_RTT_ConfigUpBuffer (1, "JScope_i2i2i2i2", rtt_buf1, sizeof(rtt_buf1), SEGGER_RTT_MODE_NO_BLOCK_SKIP);
	#endif

	//---------------------------------------------------------
  	adc_isr_flag = 0;
	opa_cali_flag = 0;
	sw1_flag = 0;
	mode = 0;
    //---------------------------------------------------------	

	XMC_GPIO_Init(P1_2, &OUTPUT_strong_sharp_config);	// TP
	XMC_GPIO_Init(P1_3, &OUTPUT_strong_sharp_config);	// TP
	
	XMC_GPIO_Init(P14_8, &user_switch_read_config);		// USER0 SW
	XMC_GPIO_Init(P14_9, &user_switch_read_config);		// USER1 SW

    //---------------------------------------------------------
	CCU80_init ();

	//---------------------------------------------------------
	ADC_init ();
	
  	NVIC_EnableIRQ(VADC0_G0_0_IRQn);
	
	//---------------------------------------------------------
	get_current_offset();
	
	//---------------------------------------------------------
	while (1){
		if(b_ms){//1ms
			b_ms = 0;
			
			foc_status();

			if((cnt_ms&0x07)==0x07){    //8ms
				if (XMC_GPIO_GetInput(XMC_GPIO_PORT14, 9) != key){
					if (key_delay++>5){
						key_delay = 0;
						key = XMC_GPIO_GetInput(XMC_GPIO_PORT14, 9);
						if(key==0){
							sw1_flag ^= 1;
						}
					}
				}else{
					key_delay = 0;
				}
			}
		}
	}
}

//==============================================================================

void VADC0_G0_0_IRQHandler(void) 
{
	static char segF = 0;

	//--------------2khzpwm 50us一次-------------------
	if(++cnt_main>20){
		cnt_main = 0;
		b_ms = 1;
		cnt_ms++;
	}
	//=================================================

	//怎么做滤波  
	//三相电流采样??
	//估算角速度,误差??
	//---------------------------------------------------------
	LED_BIG_BOARD_SET(1);//32us

	adc_isr_flag = 1;

	//---------------------------------------------------------
	vadc_ia   = XMC_VADC_GROUP_GetResult(VADC_G0, 0);
	vadc_ib   = XMC_VADC_GROUP_GetResult(VADC_G0, 3);
	vadc_ic   = XMC_VADC_GROUP_GetResult(VADC_G0, 4);
	vadc_vpot = XMC_VADC_GROUP_GetResult(VADC_G0, 6);//  


	if (opa_cali_flag){
		//-----------------------------------------------------
		// current measure
		//-----------------------------------------------------
		ia_mea = (int16_t)(ia_offset - vadc_ia);//
		ib_mea = (int16_t)(ib_offset - vadc_ib);
		ic_mea = (int16_t)(ic_offset - vadc_ic);
		
		//-------测一下电流正负关系----------------
		#if 0 //测量的是续流电流  测量值与实际值刚好方向相反
			if(ia_mea>0){
				LED_BIG_BOARD_SET(1);
			}else{
				LED_BIG_BOARD_SET(0);
			}
		#endif
		//========================================

		//计算出实际电流大小
		ia_mea_f = (float)ia_mea * 0.00805664062f;// 0.00805664062f (A/bit)  =  Imax / 2048   Imax = 3.3/2 / R / gain
		ib_mea_f = (float)ib_mea * 0.00805664062f;
		ic_mea_f = (float)ic_mea * 0.00805664062f;

		ia_mea_t = CUR_ADC_2_Q15(ia_mea); 
		ib_mea_t = CUR_ADC_2_Q15(ib_mea);
		ic_mea_t = CUR_ADC_2_Q15(ic_mea);

		//----------------------------------------------------- 
		// clarke transform
		//-----------------------------------------------------
		// Ialpha = (2/3) * (Ia - Ib/2 - Ic/2) = Ia  已经做了等赋值变换
		Ialpha = 0.6666666667f * (ia_mea_f - 0.5f * (ib_mea_f + ic_mea_f));
		Ialpha_t = 2 * (ia_mea_t - (ib_mea_t + ic_mea_t) / 2 ) / 3;
		// Ibeta = (1/sqrt(3)) * (Ib - Ic)
		Ibeta = 0.5773502692f * (ib_mea_f - ic_mea_f);
		Ibeta_t = (ONE_DIV_SQRT3_Q * (ib_mea_t - ic_mea_t) )>> 15;

		//-----------------------------------------------------
		// Park transform
		//-----------------------------------------------------
		sin_f = (float)arm_sin_f32( theta_est );
		cos_f = (float)arm_cos_f32( theta_est );
		Id_mea =  cos_f * Ialpha + sin_f * Ibeta; 
		Iq_mea = -sin_f * Ialpha + cos_f * Ibeta; 
		// Id_mea =  arm_cos_f32( theta_est ) * Ialpha + arm_sin_f32( theta_est ) * Ibeta; 
		// Iq_mea = -arm_sin_f32( theta_est ) * Ialpha + arm_cos_f32( theta_est ) * Ibeta; 
		sin_t = (sin_f * 32768);
		cos_t = (cos_f * 32768);
		tmp1 = ((int)cos_t * Ialpha_t)>>15;
		tmp2 = ((int)sin_t * Ibeta_t)>>15;
		Id_mea_t =  tmp1 + tmp2; 
		tmp1 = ((int)-sin_t * Ialpha_t)>>15;
		tmp2 = ((int)cos_t * Ibeta_t)>>15;
		Iq_mea_t = tmp1 + tmp2; 

		//-----------------------------------------------------
		// calc position
		//-----------------------------------------------------
		position_estimate();

		//-----------------------------------------------------		
		switch (mode){
			//-------------------------------------------------
			// check user1 switch 
			//-------------------------------------------------
			case e_idle:{

				Id_ref = 0.0f;
				Iq_ref = 0.0f;
				sum_q = 0.0f;
				sum_d = 0.0f;

				//---------------------------------------------
				if (sw1_flag){
					sw1_flag = 0;

					led_cnt = 0;
					delay_cnt = 0;

					Id_ref = 0.0f;
					Id_err = 0.0f;
					
					Iq_ref = 0.0f;
					Iq_err = 0.0f;
					
					sp_err = 0.0f;

					sum_d = 0.0f;
					sum_q = 0.0f;
					sum_sp = 0.0f;
					sum_pll = 0.0f;
					sum_omega = 0.0f;

					Vd = 0.0f;
					Vq = 0.0f;

					e_omega_est = 0.0f;
					e_omega_set = 0.0f;
					
					CCU80_IO_PWM_init ();		
					mode = 1;
				}
				//---------------------------------------------
			}
			break;
			
			//对齐-------------------------------------------------
			case e_align:{
				theta = 0.0f; //对齐0角度
				#if START_TYPE == START_BY_ID //id add
					Id_ref += STARTUP_CUR_RAMP;//
					Vq = 0;
					if(Id_ref>STARTUP_CUR_MAX){
						theta_inc = RAMP_INC_STEP;
						mode = 2;
						delay_cnt = 0;
						//对齐后  大板灯灭,小板灯亮
					}
				#elif START_TYPE == START_BY_IQ
					Id_ref = 0.0f;
					Iq_ref += STARTUP_CUR_RAMP;//
					if(Iq_ref>STARTUP_CUR_MAX){
						theta_inc = RAMP_INC_STEP;
						mode = 2;
						delay_cnt = 0;
						LED_BIG_BOARD_SET(0);
					}
				#endif
			}
			break;
			
			//强拖-----角速度递增 电流保持---------------------
			case e_open:{
				Iq_ref += IQ_INC;
				if(Iq_ref>IQ_INC_MAX){
					Iq_ref = IQ_INC_MAX;
				}

				if (++delay_cnt >= RAMP_INC_CNT){//
					delay_cnt = 0;
					
					theta_inc += RAMP_INC_STEP;//

					if (theta_inc >= MAX_INC_SPEED || e_omega_flt>SW_CL_OMEGA){
						// theta_inc = MAX_INC_SPEED;
						chk_cnt = 0;
						ramp_cnt = 0;
						//根据速度设定一个iq的值
						// Eq = 0.0f;//这里是做什么用?
						// Ed = 0.0f;//这个需要吗?

						//计算一次速度换计算出iq-ref
						#if START_TYPE == START_BY_ID
							open_2_close_cal();
						#endif

						mode = 3;
						
						#if SPEED_TYPE == 0
							ramp_val = 0;//uq 直接给定Uq没有速度环
						#elif SPEED_TYPE == 1
							#if START_TYPE == START_BY_ID //id add
								Iq_ref = 0.1f;
							#elif START_TYPE == START_BY_IQ

							#endif
						#endif
						//进入闭环,两个灯亮
					}
				}
				
				theta += theta_inc;
				if (theta >= TWO_PI){
					theta = 0.0f;
				}

				if (sw1_flag){
					sw1_flag = 0;
					
					sum_d = 0.0f;
					Vd = 0.0f;
					Vq = 0.0f;
					Id_ref = 0.0f;
					sum_pll = 0.0f;
					e_omega_est = 0.0f;
					
					XMC_GPIO_SetOutputLow(XMC_GPIO_PORT1, 3);
					CCU80_IO_GPIO_init ();
					
					mode = 0;
				}
			}
			break;
			
			// sensorless running-----------------------
			case e_close:{
				#if START_TYPE == START_BY_ID
					#if  MOTOR_SEL == M_A2212
						if(Id_ref>0){
							Id_ref -= 0.01;
						}else{
							Id_ref = 0;
						}	
					#elif MOTOR_SEL == CUR
						if(Id_ref>0){
							Id_ref -= 0.005f;
						}else{
							Id_ref = 0.0f;
						}	
					#else
						if(Id_ref>0){
							Id_ref -= 0.01f;
						}else{
							Id_ref = 0.0f;
						}	
					#endif
				#endif

				#if SPEED_TYPE == 0//电位器直接赋值Uq
					if (++ramp_cnt == VQ_RAMP_CNT){
						ramp_cnt = 0;

						if (vadc_vpot > ramp_val){		//
							ramp_val++;
						}else if (vadc_vpot < ramp_val){
							ramp_val--;
						}
					}
					Vq = (395.0f + ramp_val * 3700.0f / 4095.0f) * 6.92820323f / 4096.0f;
					// Vq = (ramp_val * MAX_UQ / 4095.0f);//直接给定
				#elif SPEED_TYPE == 1//速度环输出Uq
					if (++ramp_cnt == VQ_RAMP_CNT){
						ramp_cnt = 0;

						if (Iq_ref > omega_val){		//
							Iq_ref-=0.0002f;
						}else if (Iq_ref < omega_val){
							Iq_ref+=0.0002f;
						}
					}
				#elif SPEED_TYPE == 2//Iq pid 环

				#endif

				theta = theta_est;
				
				//检测是否堵住  按键---------------------------------------------
				if (++chk_cnt >= 50000){
					if (Eq < 0.2f){
						mode = 0;
						sw1_flag = 0;
						sum_d = 0.0f;
						Vd = 0.0f;
						Vq = 0.0f;
						Id_ref = 0.0f;
						sum_pll = 0.0f;
						e_omega_est = 0.0f;
						
						XMC_GPIO_SetOutputLow(XMC_GPIO_PORT1, 3);
						CCU80_IO_GPIO_init ();
					}
				}
				if(Eq>0.2f){
					chk_cnt = 0;
				}
				//---------------------------------------------				
				if (sw1_flag){
					sw1_flag = 0;
					
					sum_d = 0.0f;
					Vd = 0.0f;
					Vq = 0.0f;
					Id_ref = 0.0f;
					sum_pll = 0.0f;
					e_omega_est = 0.0f;
					
					XMC_GPIO_SetOutputLow(XMC_GPIO_PORT1, 3);
					CCU80_IO_GPIO_init ();
					
					mode = 0;
				}
			}
			break;
			//-------------------------------------------------
			default:{
				mode = 0;
				sw1_flag = 0;
				sum_d = 0.0f;
				sum_pll = 0.0f;
				e_omega_est = 0.0f;
				
				XMC_GPIO_SetOutputLow(XMC_GPIO_PORT1, 3);
				CCU80_IO_GPIO_init ();
			}
		}
		
		//-----------------------------------------------------
		// current control
		//-----------------------------------------------------
		Id_err = Id_ref - Id_mea;//
		d_axis_current_loop();//

		Iq_err = Iq_ref - Iq_mea;//
		q_axis_current_loop();

		dq_cicle_limit();
		//-----------------------------------------------------

		//-----------------------------------------------------
		Valpha = arm_cos_f32(theta) * Vd - arm_sin_f32(theta) * Vq; 
		Vbeta  = arm_sin_f32(theta) * Vd + arm_cos_f32(theta) * Vq; 
		
		// svpwm modulation
		svpwm ();	
		//-----------------------------------------------------
	}

	#if SEGGER_EN == 1
		segF ^= 1;
		if(segF){

			varbuf.var1 = (short)(Iq_mea * 1000);
			varbuf.var2 = (short)(Id_mea * 1000);
			// varbuf.var1 = (short)(Ialpha_t);
			// varbuf.var2 = (short)(Iq_mea_t);
			varbuf.var3 = (short)(Iq_mea_t);
			varbuf.var4 = (short)(Id_mea_t);;
			
			SEGGER_RTT_Write (1, &varbuf, 8);
		}

	#endif

	LED_BIG_BOARD_SET(0);

}

